
import  wiringpi as gpio

BEATDETECTOR_INIT_HOLDOFF=2000
BEATDETECTOR_MASKING_HOLDOFF=200
BEATDETECTOR_BPFILTER_ALPHA =0.6
BEATDETECTOR_MIN_THRESHOLD=20
BEATDETECTOR_MAX_THRESHOLD =800
BEATDETECTOR_STEP_RESILIENCY=30
BEATDETECTOR_THRESHOLD_FALLOFF_TARGET =0.3
BEATDETECTOR_THRESHOLD_DECAY_FACTOR=0.99
BEATDETECTOR_INVALID_READOUT_DELAY=2000
BEATDETECTOR_SAMPLES_PERIOD=10
#state
BEATDETECTOR_STATE_INIT=0
BEATDETECTOR_STATE_WAITING=1
BEATDETECTOR_STATE_FOLLOWING_SLOPE=2
BEATDETECTOR_STATE_MAYBE_DETECTED=3
BEATDETECTOR_STATE_MASKING=4

class BeatDetector():
    def __init__(self):
        self.Beatstate=BEATDETECTOR_STATE_INIT
        self.threshold=BEATDETECTOR_MIN_THRESHOLD
        self.beatPeriod=0
        self.lastMaxValue=0
        self.tsLastBeat=0
        self.beatDetected=0
        self.delta = 0

    def getCurrentThreshold(self):
        return self.threshold

    def getRate(self):
        if(self.beatPeriod!=0):
            return 1/self.beatPeriod*1000*60
        else:
            return 0
    def checkForBeat(self,sample):
        self.beatState(sample)


    def beatState(self,sample):
        res = {
            0:
                self.get_init(sample),
            1:
                self.get_wating(sample),
            2 :
                self.get_follwing_slope(sample),
            3 :
                self.get_maybe_detected(sample),
            4:
                self.get_masking(sample)
        }
    def get_init(self,sample):
        if(gpio.millis()>BEATDETECTOR_INIT_HOLDOFF):
            sample=BEATDETECTOR_INIT_HOLDOFF

    def get_wating(self,sample):
        if(sample>self.threshold):
            self.threshold = min(sample,BEATDETECTOR_MAX_THRESHOLD)
            sample= BEATDETECTOR_STATE_FOLLOWING_SLOPE

        if(gpio.millis()-self.tsLastBeat>BEATDETECTOR_INVALID_READOUT_DELAY):
            self.beatPeriod = 0
            self.lastMaxValue = 0
        self.decreaseThrshold()

    def get_follwing_slope(self,sample):
        if(sample<self.threshold):
            sample=BEATDETECTOR_STATE_MAYBE_DETECTED
        else:
            self.threshold = min(sample,BEATDETECTOR_MAX_THRESHOLD)
    def get_maybe_detected(self,sample):
        if(sample+BEATDETECTOR_STEP_RESILIENCY<self.threshold):
            self.beatDetected=gpio.millis()
            self.lastMaxValue=sample
            sample=BEATDETECTOR_STATE_MASKING
            self.delta =gpio.millis()-self.tsLastBeat
            if(self.delta):
                self.beatPeriod = BEATDETECTOR_BPFILTER_ALPHA*self.delta+(1-BEATDETECTOR_BPFILTER_ALPHA)*self.beatPeriod
            self.tsLastBeat = gpio.millis()
        else:
         sample=BEATDETECTOR_STATE_FOLLOWING_SLOPE

    def get_masking(self,sample):
        if(gpio.millis()-self.tsLastBeat>BEATDETECTOR_MASKING_HOLDOFF):
            sample = BEATDETECTOR_STATE_WAITING
        self.decreaseThrshold()


    def decreaseThrshold(self):
        if(self.lastMaxValue>0 and self.beatPeriod>0):
            self.threshold-=self.lastMaxValue*(gpio.millis()-BEATDETECTOR_THRESHOLD_FALLOFF_TARGET)/(self.beatPeriod/BEATDETECTOR_SAMPLES_PERIOD)
        else:
            self.threshold*=BEATDETECTOR_THRESHOLD_DECAY_FACTOR
        if(self.threshold<BEATDETECTOR_MIN_THRESHOLD):
            self.threshold=BEATDETECTOR_MIN_THRESHOLD